#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include "costmap_compress/CompressedOccupancyGrid.h"
#include <mutex>

class CostmapCompressor {
    public:
        CostmapCompressor();
        ~CostmapCompressor();
        void run();

    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;

    private:
        std::string input_costmap_compress_topic;
        std::string output_costmap_compress_topic;

    private:
        ros::Subscriber costmap_sub_;
        ros::Publisher costmap_compress_pub_;
        ros::Timer timer_; 
        costmap_compress::CompressedOccupancyGrid latest_msg_;
        std::mutex msg_mytex_;
        bool has_new_data_ = false;
    
    private:
        void process(const ros::TimerEvent& e);
        void compress_callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
    

};

class CostmapDecompressor {
    public:
        CostmapDecompressor();
        ~CostmapDecompressor();
        void run();
    
    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
        
    private:
        std::string input_costmap_decompress_topic;
        std::string output_costmap_decompress_topic;

    private:
        ros::Subscriber costmap_decompress_sub_;
        ros::Publisher costmap_pub_;
    
    private:
        void decompress_callback(const costmap_compress::CompressedOccupancyGrid::ConstPtr& msg);   
};